In applications such as robotics, computer animation, spacecraft control using Control Moment Gyros (CMG) and others, a variable matrix that is a function of a variable parameter, for example (q) or time, and hence varies with time, connects input space to output space.
Let (x) be used to represent a vector of Cartesian coordinates describing the position and the configuration of an end effector, for example 14 in FIG. 1. For a three dimensional Cartesian space, (x) would have one or more components to represent position and angular rotation. The size (i.e. number of dimensions) of the end-effector space is denoted as (m). For example if the end effector can perform only a three dimensional displacement, then (m=3).
Let (q) be used to describe the joint coordinates and their configuration, for example for joints 11, 12 and 13 in FIG. 1. Each joint can have one or more degrees of freedom. The total number of degrees of freedom in (q) is denoted as (n). For example, if each of the joints in FIG. 1, i.e. 11, 12 and 13, has two degrees of freedom, the total number of degrees of freedom is (n=6).
Then the model of the structure, a manipulator or a computer animation object, can be described asx=f(q)  (1a)In (Eq.1a), f(q) is a nonlinear function of the joint variables (q).
In many applications it is necessary to compute the joint variables (q) for a given set of desired end effector positions (x). This requires inverting (Eq.1a)q=[f(x)]^−1  (1b)In (Eq.1b), ([f(x)]^−1) represents the inverse of the mapping from (q) to (x).
Solving (Eq.1b) analytically is a tedious task. A numerical method for solving (Eq.1a) involves differentiating (Eq.1a) from both sides. This gives the kinematic relationshipdx=J(q)dq  (1c)
In (Eq.1c) (dq) defines the output space, (dx) defines the input space and (J(q)) is a parameter dependent Jacobian matrix that is computed by differentiating (f(q)) with respect to (q)J(q)=diff(f(q))/diff(q)  (1d)(Eq.1d), represents the differential of (f(q)) with respect to (q).
In robotics and animation of articulated figures, (x) denotes a vector constructed from angular and linear displacements defined in Cartesian space. (q) is used to describe the joint coordinates and their configurations. (dx) describes velocities defined in Cartesian space. (dq) describes the joint velocities.
In attitude control using Control Moment Gyro mechanisms, (x) is used to describe the total angular momentum of the CMG cluster as known by a skilled person, (q) is constructed by the gimbal angles, (dx) describes components of torque. (dq) denotes the gimbal rates as known by a skilled person.
The dimension (i.e. number of dimensions) of (x) and (dx) is (m).
The dimension of (q) and (dq) is (n).
Typically (m) is less or equal to (n).
For redundant configurations, (m) is less than (n).
In some embodiments of this invention, (m) and (n) can be arbitrarily large.
FIG. 1 is an example of a structure, for example a robot manipulator with 3 joints, 11, 12, 13 and one end-effector, 14. The target trajectory for the end-effector is specified as 15 in FIG. 1 and is defined in three dimensional Cartesian space. In FIGS. 1, 11, 12 and 13 are joint mechanisms that can change angularly or linearly to provide degrees of freedom (n) and to allow 14 to follow the target 15. Each of the joints 11, 12 and 13 can have one, or more degrees of freedom, and the joints can have different numbers of degrees of freedom.
FIG. 2 is a graphic structure that is used in fields such as computer animation. The particular example of the structure in FIG. 2 has five end effectors, 211, 214, 215, 213 and 212 and ten joints, 21, 22, 23, 24, 25, 26, 27, 28, 29, 210. The structure in FIG. 2 is included only for demonstrative purposes and the IK algorithm described in this invention can work on a structure with (m) degrees of freedom for the end-effectors and (n) degrees of freedom for the joints where (m) and (n) can be arbitrarily large. 216, 217, 218, 219 and 220 are the target trajectories defined in a three-dimensional Cartesian space that the end effectors have to follow.
In robotics, motors or other mechanisms are used to provide linear and or angular momentum to produce displacement and change (q).
In computer graphics, the animation software redraws the figure to produce displacement in (q).
Since the Jacobian in (Eq.1c) depends on (q), a sensor is used to measure (q) or a mathematical model is used to compute, derive or estimate (q).
In this description of the present invention, (dxd) is used to denote a vector of desired values for (dx). (dxd) and (q) are assumed known and provided by a mathematical model or a sensor.
For a given set of desired trajectories (dxd), the inverse kinematics problem (IK) is the problem of solving the inverse of (Eq.1c), that is for a given vector (dxd) how to derive (dq).dq=iJ(q)dxd  (2)
In Eq. 2 (iJ(q)) represents the inverse of the Jacobian in (Eq.1). If (m) is strictly less than (n), i.e. (m) is not equal to (n), pseudo-inverse methods are used to compute (iJ(q)).
In computer graphics and robotics, (dxd) denotes the desired velocities of the end effectors.
In attitude control using CMGs, (dxd) denotes the desired components of torque.
The problem of the inverse kinematics, which is the subject of the present invention, is how to select the joint variables (dq), i.e. to determine how to manipulate motors or how to redraw the structure in an animation application, such that (dx) follows the desired target trajectory (dxd).
FIG. 3 represents the inverse kinematics block, i.e. the processing system which determines the joint variables (dq) required to produce the target trajectory (dxd). The input to this block is (dxd) and the output from the block is (dq). The inverse kinematics block has to deliver (dq) for given (dxd) such that (dx) is same as (dxd) at each iteration.
Solving (Eq.2) for (dq) when (dxd) is given and (J(q)) is a time-varying parameter-dependant Jacobian matrix, is as an essential element or step in computer animation, control of robot manipulators, control of spacecraft and other applications.
Solving (Eq.2) in real time is a tedious and computationally intensive task.
An aim of some embodiments of the present invention is to derive a computationally efficient method for solving the inverse kinematics problem defined in (Eq.2).
Solving the IK problem in (Eq.2) in real-time is a numerically intensive task due to the high number of degrees of freedom (n) in (q).
Increasing the details and the degrees of freedom in the animation of articulated objects, for example, leads to an improved visual representation of the motion. This however leads to highly intensive computations due to the necessity of computing the IK problem in (Eq.2) in real time for a large number of (n).
Since the Jacobian (J(q)) in (Eq.1c) and (Eq.2) is a time-varying function due to its dependence on the parameter (q), at certain configurations, called singular states, (J(q)) becomes rank deficient and as a result the inverse in (Eq.2) can lead to arbitrarily large values for (dq) for a given trajectory (dxd). This is another complication associated with the computation of the inverse kinematics problem.
Traditional IK methods that derive (iJ(q)), i.e. the inverse of the Jacobian, are based on the manipulation of matrices which makes the process highly computationally intensive and difficult to run on a parallel processor architecture.
The damped least squares algorithm (DLS), also known as the singularity robust (SR) algorithm, is traditionally used to solve the problem in (Eq.2) (Y. Nakamura and H. Hanafusa, “Inverse kinematic solutions with singularity robustness for robot manipulator control”, Journal of Dynamic systems, Measurements and Control, Vol. 108, September 1986, C. W. Wampler and L. J. Leifer, “Applications of damped least-squares methods to resolved-rate and resolved-acceleration control of manipulators”, Journal of Dynamic Systems, Measurement, and Control, 110 (1988), pp. 31-38).iJ(q)=Jt(q)[J(q)Jt(q)+kI]^−1  (3a)(Jt(q)) is the transpose of the Jacobian. With the form for computing (iJ(q)) defined in (Eq.3a), one can derive (dq) for a given vector (dxd) from (Eq.2), i.e.dq=Jt(q)[J(q)Jt(q)+kI]^−1dxd  (3b)
In (Eq.3b) (Jt(q)) is the transpose of the Jacobian defined in (Eq.1d), (I) is the identity matrix, (k) is known as a damping factor that needs to be adapted and (^−1) represents the inverse operator. When (k=0), (Eq.3) reduces to the pseudo inverse methodiJ(q)=Jt(q)[J(q)Jt(q)]^−1  (4a)
With the form for computing (iJ(q)) defined in (Eq.4a), using (Eq.2) one can derive (dq) for a given vector (dxd), i.e.dq=Jt(q)[J(q)Jt(q)]^−1dxd  (4b)
At singular states the Jacobian becomes rank deficient and as a result, the inverse in (Eq.4a) does not exist and can not be generated using the mathematical formula of (Eq.4a).
Furthermore, when (J(q)) is near the singular states, solutions based on (Eq.4b) lead to excessively large values for (dq). The damping factor (k) is thus used as a trade-off between exactness of the solution and feasibility of the solution. When (k=0) (Eq.3a) reduces to (Eq.4a); (k) usually is set to (k=0) when the configuration is away from singularity. Near the singularity (k>0). Therefore, the IK method in (Eq.3a) and (Eq.3b) is further complicated due to the need for the adaptation of (k). The adaptation of the damping factor (k) requires additional computation and hence processing power.
Algorithms comprising a feedback loop that uses only the transpose of the Jacobian for the control of robot manipulators have been proposed by W. A. Wolovich and H. Elliott in “A computational technique for inverse kinematics”, Proceedings of the 23rd Conference on Decision and Control, 1984 and by A. Balestrino, G. De Maria and L. Sciavicco in “Robust control of robotic manipulators”, 9th IFAC World Congress, 1984. However, unlike the method proposed in this invention, these algorithms are incapable of avoiding or escaping singular states and fail to deliver solutions when the system is in a singular state.